def process_and_save_tum_file(input_filename, output_filename):
    """
    读取 TUM 格式的文件，减去第一个数据的 xyz 值，并保存到新的文件。

    :param input_filename: str, 输入文件名
    :param output_filename: str, 输出文件名
    """
    processed_data = []

    # 读取数据并减去第一个数据的 xyz 值
    with open(input_filename, 'r') as file:
        lines = file.readlines()

        # 读取第一行以获得初始的 x, y, z
        first_line = lines[0].strip().split()
        initial_x, initial_y, initial_z = float(first_line[1]), float(first_line[2]), float(first_line[3])

        for line in lines:
            values = line.strip().split()
            timestamp = float(values[0])
            x = float(values[1]) - initial_x
            y = float(values[2]) - initial_y
            z = float(values[3]) - initial_z
            qx = float(values[4])
            qy = float(values[5])
            qz = float(values[6])
            qw = float(values[7])

            processed_data.append((timestamp, x, y, z, qx, qy, qz, qw))

    # 保存处理后的数据到新的文件
    with open(output_filename, 'w') as file:
        for data in processed_data:
            line = f"{data[0]} {data[1]} {data[2]} {data[3]} {data[4]} {data[5]} {data[6]} {data[7]}\n"
            file.write(line)

data_dir = '../log/A1/running/'
data_file = 'LegOdom_align.txt'
save_file = 'LegOdom_align_XYZ.txt'

input_filename = data_dir + data_file
output_filename = data_dir + save_file
process_and_save_tum_file(input_filename, output_filename)